from bisect import bisect_left, insort_right
from itertools import izip
from copy import deepcopy

from utils.prettyrepr import prettify_class
from utils.plotter import get_plot_target
from utils.misc import check_type
from utils.approx import Approx

from geom2d.segment import Segment


@prettify_class
class Path(object):
    """This class is used to represent the trajectory of a crane in the warehouse's x-axis."""
    __slots__ = ("times", "positions", "segments",
                 "curr_time", "curr_position",
                 "min_position", "max_position")

    def __init__(self, time=0.0, position=0.0):
        time = Approx(time)
        position = Approx(position)
        self.times = [time]
        self.positions = [position]
        self.segments = []
        self.curr_time = time
        self.curr_position = position
        self.min_position = position
        self.max_position = position

    reset = clear = __init__

    def __info__(self):
        return ("time=%s, pos=%s, npoints=%d" %
                (self.curr_time, self.curr_position, len(self.times)))

    def __len__(self):
        """Size of the path is represented by the number of points in it."""
        return len(self.times)

    def __getitem__(self, index):
        """Access a given point in the path by index, or a slice of the path."""
        if isinstance(index, slice):
            return zip(self.times[index], self.positions[index])
        return (self.times[index], self.positions[index])

    def __iter__(self):
        return izip(self.times, self.positions)

    # --------------------------------------------------------------------------
    # Path modification methods (append, extend, pop, shift_position, shift_time)
    def append(self, time, position):
        """Add a point to the path. The time must be greater than or equal to the path's current
        time. If it is equal, then the position must match the path's current position (otherwise
        an exception is raised).
        Returns True if a new point was added to the path, False otherwise."""
        time = Approx(time)
        position = Approx(position)
        curr_time = self.curr_time
        curr_position = self.curr_position
        if time <= curr_time:
            if time < curr_time:
                raise ValueError("invalid time provided (less than current time)")
            elif position != curr_position:
                raise ValueError("invalid position provided (leads to non-continuous path)")
            return False
        else:
            self.times.append(time)
            self.positions.append(position)
            self.segments.append(Segment((curr_time, curr_position), (time, position)))
            self.curr_time = time
            self.curr_position = position
            if position < self.min_position:
                self.min_position = position
            elif position > self.max_position:
                self.max_position = position
            return True

    def extend(self, points_or_path):
        """Given an iterable of (time, position) tuples, append them to the path in the given
        order."""
        if isinstance(points_or_path, Path):
            path = points_or_path
            times = iter(path.times)
            positions = iter(path.positions)
            self.append(times.next(), positions.next())
            self.times.extend(times)
            self.positions.extend(positions)
            self.segments.extend(deepcopy(segment) for segment in path.segments)
            self.curr_time = path.curr_time
            self.curr_position = path.curr_position
            self.min_position = min(self.min_position, path.min_position)
            self.max_position = max(self.max_position, path.max_position)
        else:
            for time, position in points_or_path:
                self.append(time, position)

    def pop(self):
        """Remove the last point from the path and return it."""
        if len(self.times) == 1:
            raise IndexError("cannot pop from path with a single point")
        time = self.times.pop()
        position = self.positions.pop()
        self.segments.pop()
        self.curr_time = self.times[-1]
        self.curr_position = self.positions[-1]
        if position == self.min_position:
            self.min_position = min(self.positions)
        elif position == self.max_position:
            self.max_position = max(self.positions)
        return (time, position)

    def shift_position(self, delta):
        """Apply (in-place) a shift 'delta' to all positions in the path."""
        if delta != 0.0:
            self.positions = [position + delta for position in self.positions]
            self.curr_position += delta
            self.min_position += delta
            self.max_position += delta
            for segment in self.segments:
                segment.move(dy=delta)

    def shift_time(self, delta):
        """Apply (in-place) a shift 'delta' to all times in the path."""
        if delta != 0.0:
            self.times = [time + delta for time in self.times]
            self.curr_time += delta
            for segment in self.segments:
                segment.move(dx=delta)

    # --------------------------------------------------------------------------
    # Methods for finding the position or segment(s) at a particular time
    def position_at(self, time, lo=0, hi=None):
        """Compute the position of the crane at a given time. If the argument time is greater than
        the path's current time, the crane's current position is returned (the crane is assumed to
        remain in the same position indefinitely)."""
        if time >= self.curr_time:
            return self.curr_position
        else:
            segment = self.segments_at(time, lo, hi)[0]
            return segment.y_at(time)

    def segments_at(self, time, lo=0, hi=None):
        """Given a time value, returns the segment (or two segments if the point is at the boundary
        between the two segments) which covers the specified time. Returns a list of one or two
        segments."""
        return [self.segments[i] for i in self.segment_indices_at(time, lo, hi)]

    def segment_indices_at(self, time, lo=0, hi=None):
        """Return the index of the segment (or two segments) that cover the given time. 'lo' and
        'hi' can be used to limit the scope of the search as in the bisect module. Returns a tuple
        of one or two segment indices."""
        times = self.times
        if hi is None:
            hi = len(times)
        if hi - lo < 1:
            raise ValueError("path slice is empty")
        if time < times[lo] or time > times[hi-1]:
            raise ValueError("time value outside specified path slice")
        i = bisect_left(times, time, lo, hi)
        if time == times[i]:
            # 'time' is a segment boundary
            if i == 0:
                # 'time' is equal to the path's starting time -> return only the first segment
                return (0,)
            elif i < len(times) - 1:
                # for any '0 < i < len(times)-1' (i.e. middle points of the path),
                # we return the two segments that contain the given point
                return (i-1, i)
        # either: time < times[i]                     # i.e. 'time' is in the middle of a segment
        # or: time == times[i] and i == len(times)-1  # i.e. 'time' is equal to the current time
        return (i-1,)

    def segment_indices_covering(self, start, end, lo=0, hi=None):
        """Compute the smallest and largest indices of the segments that cover the specified time
        interval. Returns a pair of indices (i, j) that can be used with xrange (don't forget to
        add 1 to j to include index j)."""
        if start > end:
            raise ValueError("interval endpoints out of order")
        times = self.times
        if hi is None:
            hi = len(times)
        if hi - lo < 1:
            raise ValueError("path slice is empty")
        if end < times[lo] or start > times[hi-1]:
            raise ValueError("time interval outside specified path slice")
        # start is before the path slice
        if start <= times[lo]:
            i = max(lo-1, 0)
        else:
            i = bisect_left(times, start, lo, hi) - 1
        # end is after the path slice
        if end >= times[hi-1]:
            j = min(hi-1, len(self.segments)-1)
        else:
            j = bisect_left(times, end, i, hi)
            if end < times[j]:
                j -= 1
        return (i, j)

    # --------------------------------------------------------------------------
    # Intersections with other paths or segments
    def intersects(self, path_or_segment, reverse=False, lo=0, hi=None):
        """Returns true if there is at least one intersection between this path and the argment
        segment or path. If 'reverse' is True, intersections are checked from right to left."""
        intersections = iter(self.intersections(path_or_segment, reverse, lo, hi))
        try:
            intersections.next()
        except StopIteration:
            return False
        return True

    def intersection(self, path_or_segment, index=0, lo=0, hi=None):
        """Returns a specific point of intersection between this path and the argument path or
        segment. The returned point is determined by index, similar to the way a list is indexed:
            a) index >= 0, returns the (index + 1)-th left-most intersection
            b) index < 0, returns the index-th right most intersection

        This should be equivalent to 'list(path.intersections(path_or_segment))[index]', but in
        a much more efficient way since (in most cases) we don't have to actually compute all
        intersections to reach the one we want.
        This method raises an IndexError (like indexing the list of intersections) if the given
        intersection does not exist, e.g. we asked for the 3rd left-most intersection but the
        segment only intersects the path twice."""
        if index < 0:
            index *= -1
            reverse = True
        else:
            index += 1
            reverse = False
        intersections = iter(self.intersections(path_or_segment, reverse, lo, hi))
        for _ in xrange(index):
            try:
                intersection = intersections.next()
            except StopIteration:
                raise IndexError("intersection index out of range")
        return intersection

    def intersections(self, path_or_segment, reverse=False, lo=0, hi=None):
        """Compute the intersections between this path and a segment or another path."""
        if isinstance(path_or_segment, Path):
            return self.path_intersections(path_or_segment, reverse, lo, hi)
        elif isinstance(path_or_segment, Segment):
            return self.segment_intersections(path_or_segment, reverse, lo, hi)
        else:
            raise TypeError("path or segment expected")

    def segment_intersections(self, segment, reverse=False, lo=0, hi=None):
        """Computes all intersections (left-to-right by default, right-to-left if 'reverse' is
        True) of the path with the argument segment. Generates (point, index) tuples, where 'index'
        is the index of the path's segment where the intersection occurs."""
        check_type(segment, Segment)
        # first we check if the segment overlaps with the path's AABB (i.e. the path's time and
        # position bounds). If the segment is completely outside the AABB, no intersection occurs.
        min_time = self.times[0]
        max_time = self.curr_time
        if (segment.x_min > max_time or
            segment.x_max < min_time or
            segment.y_min > self.max_position or
            segment.y_max < self.min_position):
            return iter(())  # we must return an iterator (i.e. an object with a next() method)
        # now we determine the minimum slice of the path that must be checked for intersections
        start, stop = self.segment_indices_covering(segment.x_min, segment.x_max, lo, hi)
        # and finally we do the actual computation of intersections
        return self._segment_intersections(segment, reverse, start, stop+1)

    def _segment_intersections(self, segment, reverse=False, start=0, stop=None):
        """A private internal method that intersects 'segment' with all of the path's segments in
        the range of indices [start, stop)."""
        if stop is None:
            stop = len(self.segments)
        step = 1
        if reverse:
            start, stop, step = stop-1, start-1, -1
        for index in xrange(start, stop, step):
            point = self.segments[index].intersection(segment)
            if point is not None:
                yield (point, index)

    def path_intersections(self, path, reverse=False, lo=0, hi=None):
        """Returns an iterator over the intersections of the two paths. Generates (point, i, j)
        tuples where 'i' and 'j' are the indices of the intersecting segments in self and the
        argument path, respectively."""
        check_type(path, Path)
        # first we check the AABBs of both paths for possible overlap.
        # no intersection occurs if their AABBs do not overlap.
        if (path.times[0] > self.curr_time or
            path.curr_time < self.times[0] or
            path.min_position > self.max_position or
            path.max_position < self.min_position):
            return
        # now we compute the slice of this path that can potentially intersect with the other
        start, stop = self.segment_indices_covering(path.times[0], path.curr_time, lo, hi)
        stop += 1
        step = 1
        # swap xrange endpoints and step if reverse is True
        if reverse:
            start, stop, step = stop-1, start-1, -1
        # used to constrain the slice of the other path that is searched
        lo2 = 0
        hi2 = len(path.times)
        for i in xrange(start, stop, step):
            for point, j in path.segment_intersections(self.segments[i], reverse, lo2, hi2):
                yield (point, i, j)
                # constrain the slice of 'path' to be searched for intersections
                if reverse:
                    hi2 = j + 2
                else:
                    lo2 = j

    def _path_intersections_naive(self, path, reverse=False):
        """A quadratic brute-force approach that intersects every segment in 'self' with every
        segment in 'path'. This is useful to validate the results of path_intersections()."""
        start0, stop0 = 0, len(self.segments)
        start1, stop1 = 0, len(path.segments)
        step = 1
        if reverse:
            start0, stop0 = stop0-1, start0-1
            start1, stop1 = stop1-1, start1-1
            step = -1
        for i in xrange(start0, stop0, step):
            s0 = self.segments[i]
            for j in xrange(start1, stop1, step):
                s1 = path.segments[j]
                point = s1.intersection(s0)
                if point is not None:
                    yield (point, i, j)

    @staticmethod
    def multipath_segment_intersections(paths, segment, reverse=False):
        """An iterator over the intersections of the given segment with a list/set of paths."""
        pqueue = []  # queue of intersection "events" sorted by x (or -x if reverse is True)
        index = 0    # index of the next event to process

        def advance(path, iterator):
            # advance the given segment_intersections() iterator and
            # put the intersection event on the priority queue
            try:
                point, segment = iterator.next()
            except StopIteration:
                return
            sortkey = point[0] if not reverse else -point[0]
            insort_right(pqueue, (sortkey, point, segment, path, iterator), lo=index)

        # fill pqueue with initial intersection from each path
        for path in paths:
            advance(path, path.segment_intersections(segment, reverse))
        # consume the intersections in order (left-to-right, ties w/ undefined order)
        while index < len(pqueue):
            _, point, segment, path, iterator = pqueue[index]
            index += 1
            if index % 1000 == 0:
                pqueue = pqueue[index:]
                index = 0
            yield point, segment, path
            advance(path, iterator)

    def line_plot(self, target=None, **kwargs):
        plt = get_plot_target(target)
        axes = plt.line_plot(self.times, self.positions, **kwargs)
        axes.scatter(self.times, self.positions, marker="x", color=kwargs.get("color", "red"))
        axes.set_xlabel("Time")
        axes.set_ylabel("Position")
        axes.grid(True)
        plt.update()
        return axes
